/*
frigOS
Licensed under GPLv3
*/
#ifndef __FIXEDPOINT_H__
#define __FIXEDPOINT_H__

#include <stdint.h>

// 16 bit 11.5
// number of integer bits
#define FP_M 11
// number of fractional bits
#define FP_N 5

#define FP_2_PI 201
#define FP_1 32
#define FP_RAD_TO_DEG 1833

//static inline int16_t fpInt(double r) { return (int16_t) ( r * (1 << FP_N) + (r >= 0 ? 0.5 : -0.5) ); };
static inline int16_t fpIntI(int16_t i) { return (int16_t) ( i * (1 << FP_N) ); };
//static inline double fpReal(int16_t i) { return (double) i / (1 << FP_N); };
static inline int16_t fpRealI(int16_t i) { return i / (1 << FP_N); };
static inline int16_t fpAdd(int16_t a, int16_t b) { return a + b; };
static inline int16_t fpSub(int16_t a, int16_t b) { return a - b; };
static inline int16_t fpMul(int16_t a, int16_t b) { return (int16_t) ( ( (int32_t) a * (int32_t) b ) >> FP_N ); };
static inline int16_t fpDiv(int16_t a, int16_t b) { return (int16_t) ( ( (int32_t) a << FP_N ) / b ); };

#endif
